#include <iostream>
#include <memory>

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h> // 点云基本类型 pcl::PointXYZ
#include <pcl/filters/passthrough.h> // 直通滤波

#include <pcl/filters/voxel_grid.h>  // 体素滤波
#include <pcl/PCLPointCloud2.h>   // pcl::PCLPointCloud2

#include <pcl/filters/statistical_outlier_removal.h> // 滤除外点

#include <pcl/ModelCoefficients.h>             //模型系数头文件
#include <pcl/filters/project_inliers.h>          //投影滤波类头文件

#include <pcl/visualization/cloud_viewer.h>

#include <pcl/conversions.h>

#include <pcl/common/transforms.h> // 算质心

typedef pcl::PointXYZRGB PointT;
// typedef pcl::PointXYZ PointT;

class PointCloudOperation
{
    public:
        pcl::PointCloud<PointT>::Ptr cloud;
        
        pcl::PCLPointCloud2::Ptr cloud_load;
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_load2;
        
    
    public:
        // 初始化填充点云
        PointCloudOperation()
        {
            cloud = pcl::PointCloud<PointT>::Ptr(new pcl::PointCloud<PointT>);
            cloud->width = 20000;
            cloud->height = 1;
            cloud->points.resize(cloud->width * cloud->height);

            cloud_load = pcl::PCLPointCloud2::Ptr(new  pcl::PCLPointCloud2);
            cloud_load2 = pcl::PointCloud<pcl::PointXYZ>::Ptr (new pcl::PointCloud<pcl::PointXYZ>);
        }  

        void set_data(pcl::PointCloud<PointT>::Ptr& cloud_in)
        {
            for (size_t i=0; i < cloud_in->points.size(); ++i)   // 填充数据
            {   
                //rand()不需要参数，它会返回一个从0到最大随机数的任意整数
                cloud_in->points[i].x = 1024 * rand() / (RAND_MAX + 1.0f) * 10; // -1到1之间 * 10
                cloud_in->points[i].y = 1024 * rand() / (RAND_MAX + 1.0f) * 10;
                cloud_in->points[i].z = 1024 * rand() / (RAND_MAX + 1.0f) * 10;
                cloud_in->points[i].r =  rand() % 255;
                cloud_in->points[i].b =  rand() % 255;
                cloud_in->points[i].g =  rand() % 255;
            }

            // pcl::copyPointCloud(*cloud_in, *cloud_out);
        }

        void load_pointcloud(const std::string pcd_path)
        {
            pcl::PCDReader reader;
            reader.read(pcd_path, *cloud_load);  // 传解引用
            reader.read(pcd_path, *cloud_load2);
        } 

        // 直通滤波PassThrough
        void passthrough_filter(pcl::PointCloud<PointT>::Ptr& cloud_in, pcl::PointCloud<PointT>::Ptr& cloud_out)
        {
            pcl::PassThrough<PointT> pass;  // 创建直通滤波器对象
            pass.setInputCloud(cloud_in);             // 设置输入对象,传地址
            pass.setFilterFieldName("z");             // 设置过滤时所需要点云类型的Z字段
            pass.setFilterLimits(-5.0, 0.0);           // 设置过滤字段的范围
            //pass.setFilterLimitsNegative (true);   //设置保留范围内还是过滤掉范围内
            pass.filter(*cloud_out);                //执行滤波，保存过滤结果在cloud_out，传解引用
        }

        // 体素滤波VoxelGrid
        void voxel_grid(pcl::PointCloud<PointT>::Ptr& cloud_in, pcl::PointCloud<PointT>::Ptr& cloud_out)
        {
            pcl::VoxelGrid<PointT> sor; // 创建体素滤波器
            sor.setInputCloud(cloud_in);          // 输入点云
            // sor.setLeafSize(0.01f, 0.01f, 0.01f); //设置滤波时创建的体素体积为1cm的立方体,m
            sor.setLeafSize(2, 2, 2);
            sor.filter(*cloud_out);
        }
        
        // statisticalOutlierRemoval滤波器移除离群点
        // 创建滤波器，对每个点分析的临近点的个数设置为50 ，并将标准差的倍数设置为1  这意味着如果一
        //个点的距离超出了平均距离一个标准差以上，则该点被标记为离群点，并将它移除，存储起来
        void outlier_removal(pcl::PointCloud<PointT>::Ptr& cloud_in, pcl::PointCloud<PointT>::Ptr& cloud_out)
        {
            pcl::StatisticalOutlierRemoval<PointT> sor;
            sor.setInputCloud(cloud_in);
            sor.setMeanK(50);
            sor.setStddevMulThresh(1.0);
            sor.filter(*cloud_out);
        }

        // 使用参数化模型投影点云
        //将点投影到一个参数化模型上（平面或者球体等），
        //参数化模型通过一组参数来设定，对于平面来说使用其等式形式。
        //在PCL中有特定存储常见模型系数的数据结构

        void model_coefficients(pcl::PointCloud<PointT>::Ptr& cloud_in, pcl::PointCloud<PointT>::Ptr& cloud_out)
        {   
            // 填充ModelCoefficients的值,使用ax+by+cz+d=0平面模型，其中 a=b=d=0,c=1 也就是X——Y平面
            //定义模型系数对象，并填充对应的数据
            pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
            coefficients->values.resize(4);
            coefficients->values[0] = 0;
            coefficients->values[1] = 0;
            coefficients->values[2] = 1.0;
            coefficients->values[3] = 0;

            // 创建ProjectInliers对象，使用ModelCoefficients作为投影对象的模型参数
            pcl::ProjectInliers<PointT> proj; //创建投影滤波对象
            proj.setModelType(pcl::SACMODEL_PLANE); //设置对象对应的投影模型
            proj.setInputCloud(cloud_in);   //设置输入点云
            proj.setModelCoefficients(coefficients); //设置模型对应的系数
            proj.filter(*cloud_out);   //投影结果存储
        }


        void view_cloud(pcl::PointCloud<PointT>::Ptr& cloud_src, pcl::PointCloud<PointT>::Ptr& cloud_new)
        {
            pcl::visualization::PCLVisualizer viewer("Cloud Viewer");
            int v1(0);  //创建左窗口显示裁剪前点云
            viewer.createViewPort(0, 0, 0.5, 1.0, v1);
            viewer.setBackgroundColor(0, 0, 0, v1);
            viewer.addText("Original Cloud", 2, 2, "Original Cloud", v1);
            pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb1(cloud_new);
	        viewer.addPointCloud<PointT>(cloud_src, rgb1, "original cloud", v1);
	        viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "original cloud", v1);
	        viewer.addCoordinateSystem(10.0, "original cloud", v1);

            int v2(1);  //创建右窗口显示裁剪后点云
	        viewer.createViewPort(0.5, 0, 1.0, 1.0, v2);  //左右窗口大小划分，1:1
	        viewer.setBackgroundColor(0.3, 0.3, 0.3, v2);
	        viewer.addText("Trimed Cloud", 2, 2, "Trimed Cloud", v2);
            pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb2(cloud_new);
	        viewer.addPointCloud<PointT>(cloud_new, rgb2, "trimed cloud", v2);
	        viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "trimed cloud", v2);
	        viewer.addCoordinateSystem(10.0, "trimed cloud", v2);
            
            viewer.spin(); //循环不断显示点云
        }

    void view_xyz(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_src)
    {
        pcl::visualization::PCLVisualizer viewer("Cloud Viewer");
        viewer.setBackgroundColor(0, 0, 0); // 设置背景颜色setBackgroundColor

        // 显示点云自带的颜色信息 
        // pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud);

        // 根据点云的某个字段进行上色,常用的字段有：x,y,z,normal_x(X方向上的法线),normal_y,normal_z,rgb,curvature曲率
        // pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> rgb(cloud_src, "y");
        
        // 自定义单一颜色（PointCloudColorHandlerCustom）
        pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> rgb(cloud_src, 0, 255, 0); 
        // pcl::visualization::PointCloudColorHandlerRandom<pcl::PointXYZ> rgb(cloud_src); // 随机上色
        viewer.addPointCloud(cloud_src, rgb, "sample cloud");
        
        // 设置点云大小
        viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "sample cloud");
        
        // 设置坐标轴长度
        viewer.addCoordinateSystem(1, "sample cloud");
        viewer.initCameraParameters();
        
        Eigen::Vector4f centroid;
        pcl::compute3DCentroid(*cloud_src, centroid);  // 质心,齐次坐标
        pcl::PointXYZ p1(centroid[0], centroid[1], centroid[2]);
        pcl::PointXYZ p2(centroid[0], centroid[1] + 0.03, centroid[2]);
        viewer.addArrow(p2, p1, 1, 0, 0, false); // 添加箭头
        viewer.spin(); //循环不断显示点云
    }
};

int main(int argc, char** argv)
{   
    std::shared_ptr<PointCloudOperation> test_1 = std::make_shared<PointCloudOperation>();
    pcl::PointCloud<PointT>::Ptr cloud_passthrough(new pcl::PointCloud<PointT>);
    pcl::PointCloud<PointT>::Ptr cloud_voxelgrid(new pcl::PointCloud<PointT>);
    pcl::PointCloud<PointT>::Ptr cloud_outlier_removal(new pcl::PointCloud<PointT>);
    pcl::PointCloud<PointT>::Ptr cloud_project(new pcl::PointCloud<PointT>);

    pcl::PCLPointCloud2::Ptr cloud_msg (new pcl::PCLPointCloud2);

    test_1->set_data(test_1->cloud);
    test_1->passthrough_filter(test_1->cloud, cloud_passthrough);
    // test_1->view_cloud(test_1->cloud, cloud_passthrough);

    test_1->voxel_grid(test_1->cloud, cloud_voxelgrid);
    test_1->view_cloud(test_1->cloud, cloud_voxelgrid);

    test_1->outlier_removal(cloud_voxelgrid, cloud_outlier_removal);
    // test_1->view_cloud(cloud_voxelgrid, cloud_outlier_removal);

    test_1->model_coefficients(cloud_voxelgrid, cloud_project);
    // test_1->view_cloud(cloud_voxelgrid, cloud_project);

    // // 转换为模板点云
    // pcl::toPCLPointCloud2(*cloud_voxelgrid, *cloud_msg);

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_2(new pcl::PointCloud<pcl::PointXYZ>);
    test_1->load_pointcloud("../data/result.pcd");
    
    pcl::fromPCLPointCloud2(*test_1->cloud_load, *cloud_2);
    test_1->view_xyz(cloud_2);

    return 0;
}
